#include "MyConfig.h"

void MyMCU_TestSetup()
{	// kiem tra trang thai 
	cbi(DDR_SETUP, PSETUP);	// DDR_SETUP = DDR_SETUP | (0 << PSETUP);
	sbi(PORT_SETUP, PSETUP);	// PORT_SETUP = PORT_SETUP | (0 << PSETUP);
	_delay_us(10);
	if (bit_is_clear(PIN_SETUP, PSETUP))
		setup_status = 1;
	else setup_status = 0;
}

void MyMCU_Init()
{
	// cho 1 luc
	_delay_ms(1000);
	sbi(DDR_LED, LED);	// DDR_LED = DDR_LED | (1 << LED);
	cbi(PORT_LED, LED);	// PORT_LED = PORT_LED | (0 << LED);
	// nhay LED 3 lan
	for (unsigned char i = 0; i < 6; i++)
	{
		PORT_LED ^= (1 << LED);
		_delay_ms(200);
	}
}

void MyOutput_Init()
{
	sbi(DDR_OUT, C0);
	sbi(DDR_OUT, C1);
	sbi(DDR_OUT, C2);
	sbi(DDR_OUT, C3);
	cbi(PORT_OUT, C0);
	cbi(PORT_OUT, C1);
	cbi(PORT_OUT, C2);
	cbi(PORT_OUT, C3);
}

void MyInput_Init()
{
	cbi(DDR_IN, D2);
	cbi(DDR_IN, D3);
	cbi(DDR_IN, D4);
	cbi(DDR_IN, D5);
	cbi(DDR_IN, D6);
	cbi(DDR_IN, D7);
	sbi(PORT_IN, D2);	// PD2 is now an input with pull-up enabled
	cbi(PORT_IN, D3);
	cbi(PORT_IN, D4);
	cbi(PORT_IN, D5);
	cbi(PORT_IN, D6);
	cbi(PORT_IN, D7);
}

void MyI2C_Init()
{
	#define I2C_SPEED 100000L     //100kHz normal mode, this value must be used for a genuine WMP
	cbi(DDR_SDA, SDA);	// in
	sbi(DDR_SCL, SCL);	// out
	cbi(PORT_SDA, SDA);
	cbi(PORT_SCL, SCL);
	PORTD &= ~(1<<0); PORTD &= ~(1<<1);	// I2C_PULLUPS_DISABLE

	TWSR = 0;                                    // no prescaler => prescaler = 1
	TWBR = ((F_CPU / I2C_SPEED) - 16) / 2;   // change the I2C clock rate
	TWCR = 1<<TWEN;                              // enable twi module, no interrupt
}

void MyUSART_Init()
{
	cbi(DDR_RX, RX);
	sbi(DDR_TX, TX);
	cbi(PORT_RX, RX);
	cbi(PORT_TX, TX);
	// USART initialization
	// Communication Parameters: Baud Rate: 115200, 8 Data, 1 Stop, No Parity //0x98
	UCSR0A = 0x00;
	UCSR0B = 0x98; //98
	UCSR0C = 0x06;
	UBRR0H = 0x00;
	UBRR0L = 0x08;
}

void MyTimer0_Init()
{
	// Timer 0 initialization
	TIMSK0 = 0;								// Disable interrupts 
	TCCR0A = 0x00;
	TCCR0B |= (1 << CS01) | (1 << CS00);	// prescaler = clkIO/64
	TCNT0 = TCNT0_SET_1000MICROSEC;
	TIMSK0 |= (1 << TOIE0);					// cho phep ngat tran T/C0
}

void MyTimer1_Init()
{
	// Timer 1 - rotor
	TIMSK1 = 0;								// Disable interrupts 
	TCCR1A = 0;								// normal counting mode 
	TCCR1B = (1 << CS11);					// Set prescaler of 8 (8 nhip tang TCNT2 1 lan = 0.5us)
	// we use OCR1A to generate an interrupt every time TCNT1 == OCR1A.
	// Notice we do not reset TCNT1.  We just reincrease OCR1A
	ATOMIC_BLOCK(ATOMIC_FORCEON)
	{
		OCR1A = TCNT1 + TCNT1A_MIN;
	}
	TIMSK1 = (1 << OCIE1A);				 // enable compare interrupt on TCNT1 == OCR1A
}
/*
void MyTimer1_Init_Setup_Mode()
{
	// Timer 1 - rotor
	TIMSK1 = 0;								// Disable interrupts 
	TCCR1A = 0;								// normal counting mode 
	TCCR1B = (1 << CS11);					// Set prescaler of 8 (8 nhip tang TCNT2 1 lan = 0.5us)
	TCNT1 = 0;
	TIMSK1 = (1 << TOIE1);					// enable over interrupt on TCNT1 == OCR1A
}*/

/*void MyCapture_Init()
{
//	EICRA = 0x00;
//	EIMSK = 0x00;
	PCICR = (1 << PCIE2);
	PCMSK2 = (1 << PCINT18);	// PD2 PIN
	PCIFR = (1 << PCIF2); 
}*/

void MyINT0_Init()
{
	// Signal change triggers interrupt
	EICRA = (0 << ISC01) | (1 << ISC00);	// Any logical change on INT0 generates an interrupt request.
	EIFR = (1 << INTF0);
	PCICR = 0x00;
	// Global Enable INT0 interrupt
	EIMSK = (1 << INT0);
}

unsigned long millis()
{
	unsigned long m;
	uint8_t oldSREG = SREG;

	// disable interrupts while we read timer0_millis or we might get an
	// inconsistent value (e.g. in the middle of a write to timer0_millis)
	cli();
	m = timer0_millis;
	SREG = oldSREG;

	return m;
}

/*unsigned long micros()
{
	unsigned long m;
	uint8_t oldSREG = SREG, t;
	
	cli();
	m = timer0_overflow_count;
#if defined(TCNT0)
	t = TCNT0;
#elif defined(TCNT0L)
	t = TCNT0L;
#else
	#error TIMER 0 not defined
#endif
  
#ifdef TIFR0
	if ((TIFR0 & _BV(TOV0)) && (t < 255))
		m++;
#else
	if ((TIFR & _BV(TOV0)) && (t < 255))
		m++;
#endif

	SREG = oldSREG;

	return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
}*/

void SerialPrint(unsigned char c)
{
	while ((UCSR0A & DATA_REGISTER_EMPTY) == 0);
	UDR0 = c;
	asm volatile("nop");//	_delay_ms(10);
}

void SerialPrintByte(unsigned char b)		// print 1 byte
{
	unsigned char b1 = (b >> 4) & 0x0f;
	unsigned char b2 = (b & 0x0f);
	unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
	unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
	SerialPrint(c1);
	SerialPrint(c2);
}

void SerialFloatPrint(float f)
{
	unsigned char * b = (unsigned char *) &f;
	for(unsigned char i = 0; i < 4; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialIntPrint(int w)
{
	unsigned char * b = (unsigned char *) &w;
	for(unsigned char i = 0; i < 2; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialInt32Print(int32_t f)
{
	unsigned char * b = (unsigned char *) &f;
	for(unsigned char i = 0; i < 4; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialPrintlnFloatArr(float * arr, unsigned char length)
{
	for(unsigned char i = 0; i < length; i++)
	{
		SerialFloatPrint(arr[i]);
		SerialPrint(',');
	}
	SerialPrint('\n');
}

void SerialPrintFloatArr(float * arr, unsigned char length)
{
	for(unsigned char i = 0; i < length; i++)
	{
		SerialFloatPrint(arr[i]);
		SerialPrint(',');
	}
}

void SerialPrintlnIntArr(int * arr, unsigned char length)
{
	for(unsigned char i = 0; i < length; i++)
	{
		SerialIntPrint(arr[i]);
		SerialPrint(',');
	}
	SerialPrint('\n');
}

void SerialPrintIntArr(int * arr, unsigned char length)
{
	for(unsigned char i = 0; i < length; i++)
	{
		SerialIntPrint(arr[i]);
		SerialPrint(',');
	}
}

float _8Bytes_To_Float(unsigned char *a)
{
	unsigned char b[4];
	for (unsigned char i = 0; i < 4; i++)
	{
		unsigned char h = (a[i * 2] < 0x3A)? a[i * 2] - 0x30 : a[i * 2] - 0x37;
		unsigned char l = (a[i * 2 + 1] < 0x3A)? a[i * 2 + 1] - 0x30 : a[i * 2 + 1] - 0x37;
		b[i] = h * 16 + l;
	}
	float val;
	memcpy(&val, b, sizeof val);
	return val;
}

int _4Bytes_To_Int(unsigned char *a)
{
	unsigned char b[2];
	for (unsigned char i = 0; i < 2; i++)
	{
		unsigned char h = (a[i * 2] < 0x3A)? a[i * 2] - 0x30 : a[i * 2] - 0x37;
		unsigned char l = (a[i * 2 + 1] < 0x3A)? a[i * 2 + 1] - 0x30 : a[i * 2 + 1] - 0x37;
		b[i] = h * 16 + l;
	}
	int val;
	memcpy(&val, b, sizeof val);
	return val;
}

unsigned char _2Bytes_To_Byte(unsigned char *a)
{
	unsigned char h = (a[0] < 0x3A)? a[0] - 0x30 : a[0] - 0x37;
	unsigned char l = (a[1] < 0x3A)? a[1] - 0x30 : a[1] - 0x37;
	return h * 16 + l;
}

/*float FourBytes_To_Float(unsigned char *_4bytes)
{
	float a = (float) _4bytes[0];
	return a;
}
*/
